function [] = ACO()
display('planning trajectories...');
global best_list
global scale
global N
global zhangai
rr_init=12;
% rr=50;
d_resolution=24;

best_list={};   %表示最终的N组从起点到终点的点集；每行是第几组的点集，因为每组长度不一样，所以用元组；
N;    %N表示有几组

i=1:N;  %分界线
jiaodu=[pi/N+(i-1)*2*pi/N];     %jiaodu直接为一个向量；只用来表示各个区域的边界角度；
xy=[];      %得到的xy为整个平面的点阵；xy的单元(每行)是【x坐标，y坐标】；

index=zeros(200,6);        %存各个分区的行号的索引；
len=zeros(6,1);       %最长6个长度，表示对应有6 groops；表示分配到最多6 groops的分区的大小；

for dom=1:4      %4个象限；
    for i=1:floor((scale-rr_init)/d_resolution)+1
        for j=1:floor((scale-rr_init)/d_resolution)+1
            switch dom
            case 1
            xy=[xy ; rr_init+(i-1)*24 , rr_init+(j-1)*24;];   %第一象限
            case 2
            xy=[xy ; -(rr_init+(i-1)*24) , rr_init+(j-1)*24;];   %第二象限
            case 3
            xy=[xy ; -(rr_init+(i-1)*24) , -(rr_init+(j-1)*24);];   %第三象限
            otherwise
            xy=[xy ; rr_init+(i-1)*24 , -(rr_init+(j-1)*24);];   %第四象限
            end
        end
    end
end

r_distance=sqrt(xy(:,1).^2+xy(:,2).^2);
theta=acos(xy(:,1)./r_distance);
c=find(xy(:,2)<=0);             %这里c是列向量
theta(c)=2*pi-theta(c);     %这里最好用2π，得到的角度才是连续；

m=size(xy,1);     %m为整个点集xy的行数；  
for i=1:N-1
    temp=find(theta>=jiaodu(i) & theta<jiaodu(i+1));     %temp为一个向量，表示该区的所有索引号；
    len(i+1)=length(temp);        %每个分区的长度len(i)；
    index(1:len(i+1), i+1 )=temp;       %将signal每列填入各个分区的点索引号；
end
i=0;
signalother=setdiff(1:m,index);    %最后一个区，这里用signalother补上；
len(i+1)=length(signalother);
index(1:len(i+1),i+1)=signalother;     

color=['b','r','c',	'm','g','k'];
for i=2:N
    temp=xy( index(1:len(i) , i ) , : );      %这里指index取出指标，在xy中找【x，y】的行；
    scatter(temp(:,1),temp(:,2),color(i))
    hold on
end
temp=xy(signalother,:);
scatter(temp(:,1),temp(:,2),color(1))
axis([-scale scale -scale scale])
hold on


%%%%%%%%%%%%%%%%%%%%蚁群算法解决TSP问题%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%初始化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
m=30;                     %蚂蚁个数50
Alpha=1;                  %信息素重要程度参数              
Beta=5;                   %启发式因子重要程度参数
Rho=0.1;                  %信息素蒸发系数
G_max=50;                %最大迭代次数200
Q=100;                    %信息素增加强度系数

%障碍物
if N==4 || N==5 || N==6
    x{1}=[3]; x{2}=[-2 -2]; x{3}=[-4 -5]; x{4}=[3];
    y{1}=[3]; y{2}=[3 2]; y{3}=[-3 -4]; y{4}=[-2];
    [~,cols]=size(x);%cols为想要的维数，因为是1×几的
else
    x{1}=[0 -1]; x{2}=[5 6]; x{3}=[-3 -2];
    y{1}=[5 5]; y{2}=[0 -1]; y{3}=[-3 -3];
    [~,cols]=size(x);%cols为想要的维数，因为是1×几的
end


zhangai=[];
for i=1:cols   %改成cols
    for j=1:length(x{i})
        if x{i}(j)>0 && y{i}(j)>0
            zhangai=[zhangai; rr_init+24*(x{i}(j)-1) , rr_init+24*(y{i}(j)-1) ];%第一象限
        elseif x{i}(j)<0 && y{i}(j)>0
            zhangai=[zhangai; -rr_init+24*(x{i}(j)) , rr_init+24*(y{i}(j)-1) ];%第二象限
        elseif x{i}(j)<0 && y{i}(j)<0
            zhangai=[zhangai; -rr_init+24*(x{i}(j)) , -rr_init+24*(y{i}(j)) ];%第三象限
        else 
            zhangai=[zhangai; rr_init+24*(x{i}(j)-1) , -rr_init+24*(y{i}(j)) ];%第四象限
        end
    end
end

zhangai

%障碍物膨胀区
zhangaipeng=[];
for i=1:size(zhangai,1)        
    signal_bool=[1 1 1 1];      %每次初始化都为1；
    for mm=1:size(zhangai,1)
        bool1= ( zhangai(i,1)-2*rr_init~=zhangai(mm,1) || zhangai(i,2)~=zhangai(mm,2) )...
                && zhangai(i,1)-2*rr_init>=-scale;
        signal_bool(1)=signal_bool(1)&&bool1;     
            
        bool2= ( zhangai(i,1)~=zhangai(mm,1) || zhangai(i,2)+2*rr_init~=zhangai(mm,2) )...
                && zhangai(i,2)+2*rr_init<=scale;
        signal_bool(2)=signal_bool(2)&&bool2;
            
        bool3= ( zhangai(i,1)+2*rr_init~=zhangai(mm,1) || zhangai(i,2)~=zhangai(mm,2) )...
                && zhangai(i,1)+2*rr_init<=scale;
        signal_bool(3)=signal_bool(3)&&bool3;
            
        bool4= ( zhangai(i,1)~=zhangai(mm,1) || zhangai(i,2)-2*rr_init~=zhangai(mm,2) )...
                && zhangai(i,2)-2*rr_init>=-scale;
        signal_bool(4)=signal_bool(4)&&bool4;
        if ~(signal_bool(1)||signal_bool(2)||signal_bool(3)||signal_bool(4))
            break       %缩短运行时间，如果都不行了尽早跳出来；
        end
    end
    if signal_bool(1)
        zhangaipeng=[zhangaipeng;zhangai(i,1)-2*rr_init,zhangai(i,2);];
    end
    if signal_bool(2)
        zhangaipeng=[zhangaipeng;zhangai(i,1),zhangai(i,2)+2*rr_init;];
    end
    if signal_bool(3)
        zhangaipeng=[zhangaipeng;zhangai(i,1)+2*rr_init,zhangai(i,2);];
    end
    if signal_bool(4)
        zhangaipeng=[zhangaipeng;zhangai(i,1),zhangai(i,2)-2*rr_init;];
    end
end

hold on%在分好区域的情况下，保持图不变继续画
plot(zhangai(:,1),zhangai(:,2),'r*')%画障碍物
hold on
plot(zhangaipeng(:,1),zhangaipeng(:,2),'b*')%画障碍物膨胀区
hold on

for ii=1:N      %主循环开始
    C=xy(index(1:len(ii),ii),:); %31个省会城市坐标
    C=setdiff(C,zhangai,'rows');%去掉障碍物
    C=setdiff(C,zhangaipeng,'rows');%去掉障碍物膨胀区


    %%%%%%%%%%%%%%%%%%%%%%%%第一步：变量初始化%%%%%%%%%%%%%%%%%%%%%%%%
    n=size(C,1);              %n表示问题的规模（城市个数）
    D=zeros(n,n);             %D表示两个城市距离间隔矩阵
    for i=1:n
        for j=1:n
            if i~=j
                D(i,j)=((C(i,1)-C(j,1))^2+(C(i,2)-C(j,2))^2)^0.5;
            else
                D(i,j)=eps;
            end
            D(j,i)=D(i,j);
        end
    end
    Eta=1./D;                    %Eta为启发因子，这里设为距离的倒数
    Tau=ones(n,n);               %Tau为信息素矩阵
    Tabu=zeros(m,n);             %存储并记录路径的生成
    NC=1;                        %迭代计数器
    R_best=zeros(G_max,n);       %各代最佳路线
    L_best=inf.*ones(G_max,1);   %各代最佳路线的长度
%     figure;%优化解
    while NC<=G_max            
        %%%%%%%%%%%%%%%%%%第二步：将m只蚂蚁放到n个城市上%%%%%%%%%%%%%%%%
        Randpos=[];
        for i=1:(ceil(m/n))
            Randpos=[Randpos,randperm(n)];
        end
        Tabu(:,1)=(Randpos(1,1:m))'; 
        %%%%%第三步：m只蚂蚁按概率函数选择下一座城市，完成各自的周游%%%%%%
        for j=2:n
            for i=1:m
                visited=Tabu(i,1:(j-1));  %已访问的城市
                J=zeros(1,(n-j+1));       %待访问的城市
                P=J;                      %待访问城市的选择概率分布
                Jc=1;
                for k=1:n
                    if length(find(visited==k))==0
                        J(Jc)=k;
                        Jc=Jc+1;
                    end
                end
                %%%%%%%%%%%%%%%%%%计算待选城市的概率分布%%%%%%%%%%%%%%%%
                for k=1:length(J)
                    P(k)=(Tau(visited(end),J(k))^Alpha)...
                        *(Eta(visited(end),J(k))^Beta);
                end
                P=P/(sum(P));
                %%%%%%%%%%%%%%%%按概率原则选取下一个城市%%%%%%%%%%%%%%%%
                Pcum=cumsum(P);
                Select=find(Pcum>=rand);
                to_visit=J(Select(1));
                Tabu(i,j)=to_visit;
            end
        end
        if NC>=2
            Tabu(1,:)=R_best(NC-1,:);
        end
        %%%%%%%%%%%%%%%%%%%第四步：记录本次迭代最佳路线%%%%%%%%%%%%%%%%%%
        L=zeros(m,1);
        
        for i=1:m
            ac=0;
            R=Tabu(i,:);
            for j=1:(n-1)
                L(i)=L(i)+D(R(j),R(j+1));
                
                dR=C(R(j+1),:)-C(R(j),:);
                for aa=1:cols%cell的维数
                    [~,hang]=size(x{aa});
                    for ab=1:hang%cell具体元素中的维数
                        test=[12+24*(x{aa}(ab)-1),12+24*(y{aa}(ab)-1)];%障碍物xy坐标
                        if (test-C(R(j),:))*dR'>=0 && (test-C(R(j+1),:))*(-dR)'>=0 ...
                                &&norm( cross( [test-C(R(j),:) 0],[dR/norm(dR) 0] ) )<=d_resolution%d为直径
                            ac=1;%ac为标志不符合的情况
                            break
                        end
                    end
                    if ac==1
                        break
                    end
                end
                if ac==1
                    break
                end
            end
            if ac==1
                L(i)=10^7;
                continue
            end
            L(i)=L(i)+D(R(1),R(n));
        end%还缺最后返程的一条线！！
        L_best(NC)=min(L);
        pos=find(L==L_best(NC));
        R_best(NC,:)=Tabu(pos(1),:);
       

        %%%%%%%%%%%%%%%%%%%%%%%%%第五步：更新信息素%%%%%%%%%%%%%%%%%%%%%%
        Delta_Tau=zeros(n,n);
        for i=1:m
            for j=1:(n-1)
                Delta_Tau(Tabu(i,j),Tabu(i,j+1))=...
                    Delta_Tau(Tabu(i,j),Tabu(i,j+1))+Q/L(i);
            end
            Delta_Tau(Tabu(i,n),Tabu(i,1))=...
                Delta_Tau(Tabu(i,n),Tabu(i,1))+Q/L(i);
        end
        Tau=(1-Rho).*Tau+Delta_Tau;
        %%%%%%%%%%%%%%%%%%%%%%%第六步：禁忌表清零%%%%%%%%%%%%%%%%%%%%%%
        Tabu=zeros(m,n);
        %%%%%%%%%%%%%%%%%%%%%%%%%历代最优路线%%%%%%%%%%%%%%%%%%%%%%%%%%
%         for i=1:n-1
%             plot([ C(R_best(NC,i),1), C(R_best(NC,i+1),1)],...
%                 [C(R_best(NC,i),2), C(R_best(NC,i+1),2)],'bo-');
%             hold on;
%         end
%         plot([C(R_best(NC,n),1), C(R_best(NC,1),1)],...
%             [C(R_best(NC,n),2), C(R_best(NC,1),2)],'ro-');  %这里是红的线
%         title(['优化最短距离:',num2str(L_best(NC))]);
%         hold off;
%         pause(0.005);
        NC=NC+1;   
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%第七步：输出结果%%%%%%%%%%%%%%%%%%%%%%%%%%
    NC=NC-1;
    hold on
    %%%%%%%%%%%这里处理一下起点%%%%%%%%%%%%
    %只用动R_best(NC,:)，不用管C；
    %找离原点最近的，直接找x^2+y^2最小的；
    [~, start_index] = min(  C( R_best(NC,:), 1 ).^2 + C( R_best(NC,:), 2 ).^2  );
    test_best = zeros(1, n);    %相当于一个中间变量；
    test_best(1 : n-start_index+1)= R_best(NC, start_index : n);
    test_best(n-start_index+2 : n)= R_best(NC, 1: start_index-1);
    R_best(NC,:) = test_best;
    best_list{ii} =  C( R_best(NC,:), : ) ;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    for i=1:n-1
        plot([ C(R_best(NC,i),1), C(R_best(NC,i+1),1)],...
            [C(R_best(NC,i),2), C(R_best(NC,i+1),2)],'b:');
        hold on;
    end
    plot([C(R_best(NC,n),1), C(R_best(NC,1),1)],...
        [C(R_best(NC,n),2), C(R_best(NC,1),2)],'r:');  %这里是红的线
%     title(['优化最短距离:',num2str(L_best(NC))]);
%     hold off;
    hold on;

    
%     Pos=find(L_best==min(L_best));
%     Shortest_Route=R_best(Pos(1),:);            %最佳路线
%     Shortest_Length=L_best(Pos(1));             %最佳路线长度
%     figure
%     plot(L_best)
%     xlabel('迭代次数')
%     ylabel('目标函数值')
%     title('适应度进化曲线')
%     ii;
end
